#include "physics/NvidiaPhysX.h"
#include "Messenger.h"
#include "Matrix44.h"
#include "Vector3.h"
#include "ConfigurationManager.h"
#include "Messenger.h"
#include "event/Events.h"
#include "Timer.h"
#include "component/Rigidbody.h"
#include "component/ColliderSphere.h"
#include "component/ColliderBox.h"
#include "component/ColliderCapsule.h"
#include "physics/SimulationCallback.h"
#include "component/Rigidbody.h"
#include "Quaternion.h"



using namespace Boon;
using namespace Physics;
using namespace physx;


PxDefaultAllocator pxDefaultAllocator;


NvidiaPhysX::NvidiaPhysX(void)
	:	m_Physics( nullptr ),
		m_Foundation( nullptr ),
		m_MaterialDefault( nullptr ),
		m_CpuDispatcher( nullptr ),
		//m_CudaContextMngr( nullptr ),
		m_Messenger( &Messenger::GetInstance() ),
		m_Sim_Callback( new SimulationCallback( m_Actors, m_GameObjects ) ),
		m_Sim_Filter_Callback( new SimulationFilterCallback() ),
		m_TimeStep( Timer::GetInstance().DeltaTimeFixed() )
{

}


NvidiaPhysX::~NvidiaPhysX(void)
{
}

PxDefaultAllocator gDefaultAllocatorCallback;


bool Boon::Physics::NvidiaPhysX::Init()
{
	ConfigurationManager& cm = ConfigurationManager::GetInstance();

	// todo -> memorypool for physx
	m_Foundation = PxCreateFoundation( PX_PHYSICS_VERSION, gDefaultAllocatorCallback, *this );
	if( !m_Foundation ) return false;

	// creating cuda context for physx calculations on gpu; nullptr -> no profiling
	/*pxtask::CudaContextManagerDesc cudaContextManagerDesc;
	try
	{
	m_CudaContextMngr = pxtask::createCudaContextManager( *m_Foundation, cudaContextManagerDesc, nullptr );
	}
	catch( std::exception& e )
	{
	const char* errmsg = e.what();
	uint_32 bla = 2;
	bla = 3;
	}
	if( m_CudaContextMngr )
	{
		if( !m_CudaContextMngr->contextIsValid() )
		{
			m_CudaContextMngr->release();
			m_CudaContextMngr = nullptr;
		}
	}*/

	// create physx with default timescale, mass, metric object size
	m_Physics = PxCreatePhysics( PX_PHYSICS_VERSION, *m_Foundation, PxTolerancesScale() );
	if( !m_Physics ) return false;

	//default material saved as member var
	m_MaterialDefault.m_Mat = m_Physics->createMaterial( .1f, .25f, .1f );
	m_MaterialDefault.SetFrictionCombineMode( FCM_MULTIPLY );
	if( !m_MaterialDefault.m_Mat )
		return false;

	// descriptor with given gravity
	PxSceneDesc sceneDescription( m_Physics->getTolerancesScale() );
	const Vector3& gravity = cm.GetGravity();
	sceneDescription.gravity = PxVec3( gravity.x, gravity.y, gravity.z );
	
	// create default physx cpu dispatcher
	if( !sceneDescription.cpuDispatcher )
	{
		m_CpuDispatcher = PxDefaultCpuDispatcherCreate( cm.GetPhysicsCpuDispatcherThreads() );
		if( !m_CpuDispatcher )
			return false;
		sceneDescription.cpuDispatcher = m_CpuDispatcher;
	}

	// set creation function ptr for simulation filter shader
	/*This shader provides the following logic: 

	- If one of the two filter objects is a trigger, the pair is acccepted and PxPairFlag::eTRIGGER_DEFAULT 
		will be used for trigger reports 
	- Else, if the filter mask logic (see further below) discards the pair it will be suppressed (PxFilterFlag::eSUPPRESS) 
	- Else, the pair gets accepted and collision response gets enabled (PxPairFlag::eCONTACT_DEFAULT) */
	if( !sceneDescription.filterShader )
	{
		sceneDescription.filterShader = SimulationFilterCallback::OnCallFilterShader;
		//sceneDescription.filterCallback = &m_Sim_Filter_Callback; -> memory violation?!
	}
	//// creating gpu-dispatcher, if a cuda-context could not be created 
	//// fall back to cpu-dispatcher
	//if( !sceneDescription.gpuDispatcher && m_CudaContextMngr )
	//{
	//	sceneDescription.gpuDispatcher = m_CudaContextMngr->getGpuDispatcher();
	//}

	// flags describing behavior when resolving collision pairs
	//sceneDescription.flags |= PxSceneFlag::eENABLE_SWEPT_INTEGRATION; //Enables a second broad phase check after integration that makes it possible to prevent objects from tunneling through eachother. 
	//sceneDescription.flags |= PxSceneFlag::eENABLE_KINEMATIC_STATIC_PAIRS ; //Enable contact pair filtering between kinematic and static rigid bodies. 
	//sceneDescription.flags |= PxSceneFlag::eENABLE_KINEMATIC_PAIRS ; //Enable contact pair filtering between kinematic rigid bodies
	sceneDescription.flags |= PxSceneFlag::eENABLE_ONE_DIRECTIONAL_FRICTION ;
	//sceneDescription.flags |= PxSceneFlag::eENABLE_TWO_DIRECTIONAL_FRICTION ; //Enable two directional per-contact friction model. 

	// finally create scene
	m_Scene = m_Physics->createScene( sceneDescription );
	if ( !m_Scene )
		return false;
	
	// create buffer for raycast queries
	//m_RayBuffer = Shared_ptr<SqRayBuffer>( new SqRayBuffer( m_Scene, 10, 10 ) );

	// set callback class to resolve collision / trigger events
	m_Scene->setSimulationEventCallback( &m_Sim_Callback );
		
	// register events
	Messenger& m = Messenger::GetInstance();
	EventListener listener( this, &NvidiaPhysX::OnRigidbodyEvent );
	m.Register( Event_Physics_Rigidbody::ID, listener );
	listener.bind( this, &NvidiaPhysX::OnColliderEvent );
	m.Register( Event_Physics_Collider::ID, listener );

	if( m_Physics->getPvdConnectionManager() == NULL)
		return false;

	// try to connect with visual debugger
	PxVisualDebuggerConnectionFlags theConnectionFlags( PxVisualDebuggerExt::getAllConnectionFlags() );
	PVD::PvdConnection* theConnection = PxVisualDebuggerExt::createConnection(m_Physics->getPvdConnectionManager(), "localhost", 5425, 10, theConnectionFlags );
	if (theConnection)
		theConnection->release();
	return true;
}

bool Boon::Physics::NvidiaPhysX::Unload()
{
	// unregister events
	Messenger& m = Messenger::GetInstance();
	EventListener listener( this, &NvidiaPhysX::OnRigidbodyEvent );
	m.Remove( Event_Physics_Rigidbody::ID, listener );
	listener.bind( this, &NvidiaPhysX::OnColliderEvent );
	m.Remove( Event_Physics_Collider::ID, listener );

	auto iter = m_Actors.begin();
	auto end = m_Actors.end();

	// release actors and joints
	while( iter != end )
	{
		iter->second.m_Actor->release();
		if( iter->second.m_JointSelfConstraint )
			iter->second.m_JointSelfConstraint->release();
		++iter;
	}
	
	//m_CudaContextMngr->release();
	m_MaterialDefault.m_Mat->release();
	m_Scene->release();
	m_CpuDispatcher->release();
	m_Physics->getPvdConnectionManager()->disconnect();
	m_Physics->release();
	m_Foundation->release();
	return true;
}

void Boon::Physics::NvidiaPhysX::SyncGameObjects()
{
	throw std::exception("The method or operation is not implemented.");
}

void Boon::Physics::NvidiaPhysX::Update()
{
	m_Messenger->Bang( IMessengerEventStrong( new Event_Physics_Update() ) );
	m_Scene->simulate( m_TimeStep );
	m_Scene->fetchResults( true );

	auto iter = m_Actors.begin();
	auto end = m_Actors.end();

	// update positions/rotations of rigidbodies
	while( iter != end )
	{
		PxRigidActor* actor = iter->second.m_Actor;
		if( actor->getType() == PxActorType::eRIGID_DYNAMIC )
		{
			iter->second.m_GameObject->GetTransform().SetPosition( actor->getGlobalPose().p );
			PxQuat& q = actor->getGlobalPose().q;
			iter->second.m_GameObject->GetTransform().SetRotation( Quaternion( q.x, q.y, q.z, q.w ) );
		}
		++iter;
	}
}


void Boon::Physics::NvidiaPhysX::AddForce( const Rigidbody& rigidbody, const Vector3& force, ForceMode mode )
{
	AddForce( rigidbody, force.x, force.y, force.z, mode );
}

void Boon::Physics::NvidiaPhysX::AddForce( const Rigidbody& rigidbody, float x, float y, float z, ForceMode mode )
{
	auto result = m_Actors.find( rigidbody.GetGameObject().GetUniqueObjectID() );
	if( result != m_Actors.end() )
	{
		PxForceMode::Enum pxMode;
		switch( mode )
		{
		case FM_IMPULSE:
			pxMode = PxForceMode::eIMPULSE;
			break;
		case FM_FORCE:
			pxMode = PxForceMode::eFORCE;
			break;
		}
		PxRigidBodyExt::addForceAtPos(	static_cast<PxRigidDynamic&>( *result->second.m_Actor ),
										PxVec3( x, y, z ), 
										result->second.m_Actor->getGlobalPose().p,
										pxMode );

	}
}

const Vector3 Boon::Physics::NvidiaPhysX::GetVelocity( const GameObjectID& id ) const
{
	auto result = m_Actors.find( id );
	if( result == m_Actors.end() || !result->second.m_Actor->isRigidDynamic() )
		return Vector3::ZERO;
	PxVec3 vel = PxRigidBodyExt::getVelocityAtPos(	static_cast<PxRigidDynamic&>( *result->second.m_Actor ),
													result->second.m_Actor->getGlobalPose().p );
	return Vector3( vel.x, vel.y, vel.z );
}

void Boon::Physics::NvidiaPhysX::SetVelocity( const GameObjectID& id, const Vector3& velocity )
{
	auto result = m_Actors.find( id );
	if( result == m_Actors.end() || !result->second.m_Actor->isRigidDynamic() )
		return;
	static_cast<PxRigidDynamic*>( result->second.m_Actor )->setLinearVelocity( velocity );
}

void Boon::Physics::NvidiaPhysX::Translate( const GameObjectID& id, const Vector3& direction )
{
	throw std::exception("The method or operation is not implemented.");
}

const Matrix44 Boon::Physics::NvidiaPhysX::GetTransform( const GameObjectID& id ) const
{
	throw std::exception("The method or operation is not implemented.");
}

void Boon::Physics::NvidiaPhysX::SetTransform( const Transform& t )
{
	GameObject& g = t.GetGameObject();
	// static actor
	PxRigidActor* actor = nullptr;

	auto result = m_Actors.find( g.GetUniqueObjectID() );
	if( result == m_Actors.end() ) return;

	// set new transform
	actor = result->second.m_Actor;
	actor->setGlobalPose( PxTransform( t.GetPosition(), t.GetRotation() ) );
}

void Boon::Physics::NvidiaPhysX::RemoveGameObject( const GameObjectID& g )
{
	throw std::exception("The method or operation is not implemented.");
}

void Boon::Physics::NvidiaPhysX::IgnoreCollision( const GameObjectID& a, const GameObjectID& b, bool enabled )
{
	throw std::exception("The method or operation is not implemented.");
}

void Boon::Physics::NvidiaPhysX::OverlapSphere( const Vector3& position, float radius, ColliderResult& outResult )
{
	throw std::exception("The method or operation is not implemented.");
}

bool Boon::Physics::NvidiaPhysX::RayCast( const Vector3& origin, const Vector3 direction, RaycastHit& outHit, float distance, bool staticOnly )
{
	PxSceneQueryFilterData queryFilterData;
	if( staticOnly )
		queryFilterData = PxSceneQueryFilterData( PxSceneQueryFilterFlag::eSTATIC );
	
	PxRaycastHit hit;
	if( !m_Scene->raycastSingle( origin, direction, distance, PxSceneQueryFlags( 0xffffffff ), hit, queryFilterData ) )
		return false;
	outHit.Point = Vector3( hit.impact.x, hit.impact.y, hit.impact.z );
	outHit.Normal = Vector3( hit.normal.x, hit.normal.y, hit.normal.z );
	outHit.Distance = hit.distance;
	outHit.GameObject = &m_GameObjects.find( ActorPtrToID( &hit.shape->getActor() ) )->second;
	return true;
}

void Boon::Physics::NvidiaPhysX::reportError( PxErrorCode::Enum code, const char* message, const char* file, int line )
{
	const char* bla = message;
}

Boon::MaterialPhysics Boon::Physics::NvidiaPhysX::CreateMaterial( float frictionSt, float frictionDyn, float restitution )
{
	if( !m_Physics )
		throw( BoonException(	"NvidiaPhysX",
		"CreateMaterial",
		"Could not create material because physics were not initialized correctly!" ) );

	return MaterialPhysics ( m_Physics->createMaterial( frictionSt, frictionDyn, restitution ) );
}

#pragma region Rigidbody Region

#define ConstraintToPXD6Motion( IsConstrained ) PxD6Motion::Enum( !(IsConstrained) << 1 )

bool Boon::Physics::NvidiaPhysX::AddRigidbody( Rigidbody& rigidbody )
{
	Transform& t = rigidbody.GetGameObject().GetTransform();
	PxRigidDynamic* actor = m_Physics->createRigidDynamic( PxTransform( t.GetPosition(), t.GetRotation() ) );
	PxD6Joint* j = PxD6JointCreate( *m_Physics, actor, actor->getGlobalPose(), nullptr, actor->getGlobalPose() );
	
	BOON_ASSERT( j != nullptr )

	// crate a dynamic rigidbody and store in map with strong ref to game object
	if( actor )
	{
		const GameObjectID id = rigidbody.GetGameObject().GetUniqueObjectID();

		// insert into 2 lists to lookup by goID or actor-ptr
		ActorInfo entry( GameObject::GetByID( id ), actor, j );
		m_Actors.insert( m_Actors.begin(), MapPairActorPerGO( id, entry ) );

		MapPairGOPerActor entry2( reinterpret_cast<std::size_t>( actor ), rigidbody.GetGameObject() );
		m_GameObjects.insert( m_GameObjects.begin(), entry2 );

		UpdateRigidbody( rigidbody );
		m_Scene->addActor( *actor );
		return true;
	}
	return false;
}

bool Boon::Physics::NvidiaPhysX::UpdateRigidbody( Rigidbody& rigidbody )
{
	auto result = m_Actors.find( rigidbody.GetGameObject().GetUniqueObjectID() );
	if( result != m_Actors.end() )
	{
		BOON_ASSERT( result->second.m_Actor->getType() == PxActorType::eRIGID_DYNAMIC )
		PxRigidDynamic* actor = static_cast<PxRigidDynamic*>( result->second.m_Actor );

		Transform& t = rigidbody.GetGameObject().GetTransform();
		actor->setGlobalPose( PxTransform( t.GetPosition(), t.GetRotation() ) );

		// use provided or default material
		MaterialPhysics material = m_MaterialDefault.m_Mat;
		Collider* collider = rigidbody.GetGameObject().GetCollider();
		if( collider )
			material = collider->GetMaterial();

		// set rigidbody flags
		actor->setAngularDamping( material.GetDampingAngular() );
		actor->setLinearDamping( material.GetDamping() );
		actor->setRigidDynamicFlag( PxRigidDynamicFlag::eKINEMATIC, rigidbody.IsKinematic() );
		actor->setActorFlag( PxActorFlag::eVISUALIZATION, true );

		PxRigidBodyExt::updateMassAndInertia( *actor, rigidbody.GetMass() );

		// update joint flags
		PxD6Joint* j = result->second.m_JointSelfConstraint;
		BOON_ASSERT( j != nullptr )

		j->setMotion( PxD6Axis::eX, ConstraintToPXD6Motion( rigidbody.IsConstrained( RIGC_FREEZE_POS_X ) ) );
		j->setMotion( PxD6Axis::eY, ConstraintToPXD6Motion( rigidbody.IsConstrained( RIGC_FREEZE_POS_Y ) ) );
		j->setMotion( PxD6Axis::eZ, ConstraintToPXD6Motion( rigidbody.IsConstrained( RIGC_FREEZE_POS_Z ) ) );
		j->setMotion( PxD6Axis::eTWIST, ConstraintToPXD6Motion( rigidbody.IsConstrained( RIGC_FREEZE_ROT_X ) ) );
		j->setMotion( PxD6Axis::eSWING1, ConstraintToPXD6Motion( rigidbody.IsConstrained( RIGC_FREEZE_ROT_Y ) ) );
		j->setMotion( PxD6Axis::eSWING2, ConstraintToPXD6Motion( rigidbody.IsConstrained( RIGC_FREEZE_ROT_Z ) ) );

		// set callback to notify on contact and contact lost
		physx::PxFilterData filter;
		filter.word0 = PxFilterFlag::eCALLBACK;
		filter.word1 = PxFilterFlag::eNOTIFY;

		// fetch shape and set flags
		const uint_32 count = actor->getNbShapes();
		PxShape** shapes = new PxShape*[count];
		actor->getShapes( shapes, count );

		for( uint_32 i = 0; i < count; ++i )
		{
			PxShape* shape = shapes[i];
			shape->setQueryFilterData( filter ); // default
			shape->setSimulationFilterData( filter );
		}
		delete [] shapes;
		return false;
	}
	return true;
}

bool Boon::Physics::NvidiaPhysX::RemoveRigidbody( const GameObjectID id )
{
	auto result = m_Actors.find( id );
	if( result != m_Actors.end() &&
		result->second.m_Actor->getType() == PxActorType::eRIGID_DYNAMIC )
	{
		m_GameObjects.erase( ActorPtrToID( result->second.m_Actor ) ); 
		result->second.m_Actor->release();
		m_Actors.erase( result );
		return true;
	}
	return false;
}

void Boon::Physics::NvidiaPhysX::OnRigidbodyEvent( const IMessengerEventStrong& edata )
{
	Event_Physics_Rigidbody* event = static_cast<Event_Physics_Rigidbody*>( edata.Get() );
	if( event->m_EventID == Event_Physics_Rigidbody::RB_CREATED )
	{
		AddRigidbody( event->m_Rigidbody );
	}
	else if( event->m_EventID == Event_Physics_Rigidbody::RB_UPDATED )
	{
		UpdateRigidbody( event->m_Rigidbody );
	}
}
#pragma endregion

#pragma region Collider Region
bool Boon::Physics::NvidiaPhysX::CreateStaticActor( const PxGeometry& geometry, const PxVec3& position, const PxQuat& rotation, const GameObjectID id, Collider& collider )
{
	PxRigidStatic* actor = m_Physics->createRigidStatic( PxTransform( position, rotation ) );

	if( actor )
	{
		// insert into 2 maps to lookup per id and actor-ptr
		ActorInfo entry( GameObject::GetByID( id ), actor, nullptr );
		m_Actors.insert( m_Actors.begin(), MapPairActorPerGO( id, entry ) );
		MapPairGOPerActor entry2( reinterpret_cast<uint_32>( actor ), collider.GetGameObject() );
		m_GameObjects.insert( m_GameObjects.begin(), entry2 );
		UpdateColliderAny( collider, geometry, position, rotation );
		m_Scene->addActor( *actor );
		return true;
	}
	return false;
}

bool Boon::Physics::NvidiaPhysX::AddColliderAny( Collider& collider, const physx::PxGeometry& geometry, const physx::PxVec3& position, const physx::PxQuat& rotation )
{
	const GameObjectID id = collider.GetGameObject().GetUniqueObjectID();

	// no rigidbody attached -> treat as static actor
	if( !collider.GetGameObject().GetRigidbody() )
		return CreateStaticActor( geometry, position, rotation, id, collider );
	else // add collider for rigidbody
		return UpdateColliderAny( collider, geometry, position, rotation );
	return false;
}


void Boon::Physics::NvidiaPhysX::OnColliderEvent( const IMessengerEventStrong& edata )
{
	Event_Physics_Collider* event = static_cast<Event_Physics_Collider*>( edata.Get() );
	if( event->m_Event == Event_Physics_Collider::C_CREATED )
	{
		AddCollider( event->m_Collider );
	}
	else if ( event->m_Event == Event_Physics_Collider::C_UPDATED )
	{
		UpdateCollider( event->m_Collider );
	}
}

bool Boon::Physics::NvidiaPhysX::AddCollider( Collider& collider )
{
	Transform& t = collider.GetGameObject().GetTransform();
	collider.SetMaterial( m_MaterialDefault ); //assign as default

	switch( collider.GetType() )
	{
	case CT_SPHERE:
		{
			float r = static_cast<ColliderSphere&>( collider ).GetRadius();
			return AddColliderAny( collider, PxSphereGeometry( r ), t.GetPosition(), t.GetRotation() );
		}
	case CT_CAPSULE:
		{
			ColliderCapsule& capsule = static_cast<ColliderCapsule&>( collider );
			float r = capsule.GetRadius(), h = capsule.GetHeight();
			// physics geometry @ half-size
			return AddColliderAny( collider, PxCapsuleGeometry( r, h * .5f ), t.GetPosition(), t.GetRotation() );
		}
	case CT_BOX:
		{
			Vector3 size = static_cast<ColliderBox&>( collider ).GetSize() * .5f;
			// physics geometry @ half-size
			return AddColliderAny( collider, PxBoxGeometry( size.x, size.y, size.z ), t.GetPosition(), t.GetRotation() );
		}
	}
	return false;
}

bool Boon::Physics::NvidiaPhysX::UpdateCollider( Collider& collider )
{
	Transform& t = collider.GetGameObject().GetTransform();
	Vector3 position = collider.GetGameObject().GetComponent<Transform>().Lock()->GetPosition();

	switch( collider.GetType() )
	{
	case CT_SPHERE:
		{
			float r = static_cast<ColliderSphere&>( collider ).GetRadius();
			return UpdateColliderAny( collider, PxSphereGeometry( r ), t.GetPosition(), t.GetRotation() );
		}
	case CT_CAPSULE:
		{
			ColliderCapsule& capsule = static_cast<ColliderCapsule&>( collider );
			float r = capsule.GetRadius(), h = capsule.GetHeight();
			// physics geometry @ half-size
			return UpdateColliderAny( collider, PxCapsuleGeometry( r, h * .5f ), t.GetPosition(), t.GetRotation() );
		}
	case CT_BOX:
		{
			// physics geometry @ half-size
			Vector3 size = static_cast<ColliderBox&>( collider ).GetSize() * .5f;
			return UpdateColliderAny( collider, PxBoxGeometry( size.x, size.y, size.z ), t.GetPosition(), t.GetRotation() );
		}
	}
	return false;
}

bool Boon::Physics::NvidiaPhysX::UpdateColliderAny( Collider& collider, const physx::PxGeometry& geometry, const physx::PxVec3& position, const physx::PxQuat& rotation )
{
	const GameObjectID id = collider.GetGameObject().GetUniqueObjectID();
	MaterialPhysics material = collider.GetMaterial();

	auto result = m_Actors.find( id ); 
	if( result == m_Actors.end() )
		return false;


	PxRigidActor* actor = result->second.m_Actor;
	
	// update dynamic actor collider
	if( collider.GetGameObject().GetRigidbody() )
	{
		PxRigidDynamic* dynActor = static_cast<PxRigidDynamic*>( actor );
		dynActor->setAngularDamping( material.GetDampingAngular() );
		dynActor->setLinearDamping( material.GetDamping() );
	}
	
	// check current shapes, if shape available update geometry
	const uint_32 count = actor->getNbShapes();
	PxShape* shape = nullptr;
	
	// add new shape with geometry specs
	if( count == 0 )
	{
		shape = actor->createShape( geometry, *material.m_Mat );
		shape->setGeometry( geometry );
	}
	else // update specific shape
	{
		PxShape** shapes = new PxShape*[count];
		actor->getShapes( shapes, count );

		for( uint_32 i = 0; i < count; ++i )
		{
			// if found correct type ( multiple attached collider?!) update specs
			if( shapes[i]->getGeometry().getType() == geometry.getType() )
			{
				shapes[i]->setGeometry( geometry );
				shape = shapes[i];
				break;
			}
		}
		delete[] shapes;
	}
	BOON_ASSERT( shape )
	
	if( collider.IsTrigger() ) // set actor to trigger
		shape->setFlags( PxShapeFlag::eSCENE_QUERY_SHAPE | PxShapeFlag::eTRIGGER_SHAPE );
	else // set it to collider
		shape->setFlags( PxShapeFlag::eSCENE_QUERY_SHAPE | PxShapeFlag::eSIMULATION_SHAPE );
	
	// set callback to notify on contact and contact lost
	physx::PxFilterData filter;
	filter.word0 = PxFilterFlag::eCALLBACK;
	filter.word1 = PxFilterFlag::eNOTIFY;
	shape->setSimulationFilterData( filter );
	shape->setQueryFilterData( filter );

	return true;
}

#pragma endregion

Boon::Physics::NvidiaPhysX::SqRayBuffer::SqRayBuffer( PxScene* scene, PxU32 numRays, PxU32 numHits )
	: m_QueryResultSize( numRays )
	, m_HitSize(numHits) 
{
	m_OrigAddresses[0] = malloc(m_QueryResultSize*sizeof(PxRaycastQueryResult) + 15);
	m_OrigAddresses[1] = malloc(m_HitSize*sizeof(PxRaycastHit) + 15);

	m_RayCastResults = reinterpret_cast<PxRaycastQueryResult*>(size_t(m_OrigAddresses[0]) + 15 & ~15);
	m_RayCastHits = reinterpret_cast<PxRaycastHit*>(size_t(m_OrigAddresses[1]) + 15 & ~15);

	PxBatchQueryDesc batchQueryDesc;
	batchQueryDesc.userRaycastResultBuffer = m_RayCastResults;
	batchQueryDesc.userRaycastHitBuffer = m_RayCastHits;
	batchQueryDesc.raycastHitBufferSize = m_HitSize;

	m_BatchQuery = scene->createBatchQuery(batchQueryDesc);
}

Boon::Physics::NvidiaPhysX::SqRayBuffer::~SqRayBuffer()
{
	m_BatchQuery->release();
	free(m_OrigAddresses[0]);
	free(m_OrigAddresses[1]);
}
